#ifndef BALANCE_CONTROLLER_H
#define BALANCE_CONTROLLER_H

#include "srpq_controller/FSM_States/ControlFSMData.h"
#include <common/Dynamics/Quadruped.h>
#include <common/Controllers/FootSwingTrajectory.h>
#include "common/cppTypes.h"
#include "srpq_controller/AdvanceControllers/BalanceController/BalanceCtrlTask.hpp"
#include "srpq_controller/AdvanceControllers/BalanceController/BC_BodyOriTask.hpp"
#include "srpq_controller/AdvanceControllers/BalanceController/BC_BodyPosTask.hpp"
#include "srpq_controller/AdvanceControllers/BalanceController/BC_FootTask.hpp"
#include "srpq_controller/AdvanceControllers/BalanceController/BC_ContactTask.hpp"

template <typename T>
class BalanceController{
public:
    BalanceController();
    virtual ~BalanceController();

    void run(void* input, ControlFSMData<T>& data);
protected:
    void _UpdateLegCMD(ControlFSMData<T>& data);
    void _ComputeBC(void* input, ControlFSMData<T>& data);

private:
    BalanceCtrlTask<T>* _body_ori_task;
    BalanceCtrlTask<T>* _body_pos_task;
    BalanceCtrlTask<T>* _foot_task;
    BalanceCtrlTask<T>* _contact_task;
    std::vector<BalanceCtrlTask<T>*> task_list; 
    BalanceCtrlData<T>* _input_data;
    PIDCtrlData<T> _pid_data;
    T stand_height = 0.28;
    
    unsigned long long _iter;
};
#endif
